#include "arm_control/arm_action_type6.hpp"
#include <chrono>
#include <thread>

//B区抓取位置

namespace arm_control
{

ArmActionType6::ArmActionType6(rclcpp::Logger logger, rclcpp::Node::SharedPtr node)
  : ArmActionBase(logger), node_(node)
{
  // 初始化MoveGroupInterface，分别为左右机械臂
  move_group_left_ = std::make_shared<moveit::planning_interface::MoveGroupInterface>(node, "arm_left");
  move_group_right_ = std::make_shared<moveit::planning_interface::MoveGroupInterface>(node, "arm_right");
  
  // 创建爪子发布器
  zhuazi_left_publisher_ = node_->create_publisher<std_msgs::msg::Int32>("/zhuazi_left", 10);
  zhuazi_right_publisher_ = node_->create_publisher<std_msgs::msg::Int32>("/zhuazi_right", 10);
  
  RCLCPP_INFO(logger_, "动作类型6初始化完成");
  RCLCPP_INFO(logger_, "MoveIt接口已初始化，Planning Group: arm_left 和 arm_right");
  RCLCPP_INFO(logger_, "爪子发布器已创建: /zhuazi_left 和 /zhuazi_right");
}

void ArmActionType6::execute(
  const std::shared_ptr<GoalHandleARM> goal_handle,
  std::shared_ptr<ARM::Feedback> feedback,
  std::shared_ptr<ARM::Result> result)
{
  RCLCPP_INFO(logger_, "执行动作类型6 - 左右臂分别移动到抓取位置1");
  
  // ========== 左臂操作流程 ==========
  
  // 更新左臂状态
  feedback->progress_percentage = 5.0;
  feedback->current_status = "更新左臂状态...";
  goal_handle->publish_feedback(feedback);
  RCLCPP_INFO(logger_, "更新左臂起始状态为当前状态...");
  move_group_left_->setStartStateToCurrentState();
  
  // 1. 先将xuan12运动到1.9199
  feedback->progress_percentage = 10.0;
  feedback->current_status = "左臂xuan12移动到1.9199...";
  goal_handle->publish_feedback(feedback);
  RCLCPP_INFO(logger_, "将左臂xuan12设置为1.9199");
  
  move_group_left_->setStartStateToCurrentState();
  
  // 获取当前状态并修改xuan12关节
  moveit::core::RobotStatePtr current_state_left = move_group_left_->getCurrentState(10.0);
  if (!current_state_left) {
    RCLCPP_ERROR(logger_, "无法获取左臂当前机器人状态");
    result->success = false;
    result->message = "动作类型6失败 - 无法获取左臂当前状态";
    goal_handle->abort(result);
    return;
  }
  
  // 获取所有关节的当前值
  std::vector<double> joint_group_positions_left;
  const moveit::core::JointModelGroup* joint_model_group_left = 
    current_state_left->getJointModelGroup("arm_left");
  current_state_left->copyJointGroupPositions(joint_model_group_left, joint_group_positions_left);
  
  // 只修改xuan12的值
  const std::vector<std::string>& joint_names_left = joint_model_group_left->getVariableNames();
  for (size_t i = 0; i < joint_names_left.size(); i++) {
    if (joint_names_left[i] == "xuan12") {
      joint_group_positions_left[i] = 1.9199;
      RCLCPP_INFO(logger_, "找到xuan12关节，设置为1.9199");
      break;
    }
  }
  
  move_group_left_->setJointValueTarget(joint_group_positions_left);
  
  moveit::planning_interface::MoveGroupInterface::Plan xuan12_plan;
  bool xuan12_success = (move_group_left_->plan(xuan12_plan) == moveit::core::MoveItErrorCode::SUCCESS);
  
  if (!xuan12_success) {
    result->success = false;
    result->message = "动作类型6失败 - 左臂xuan12路径规划失败";
    goal_handle->abort(result);
    RCLCPP_ERROR(logger_, "左臂xuan12路径规划失败！");
    return;
  }
  
  RCLCPP_INFO(logger_, "左臂xuan12路径规划成功");
  
  auto xuan12_execute_result = move_group_left_->execute(xuan12_plan);
  if (xuan12_execute_result != moveit::core::MoveItErrorCode::SUCCESS) {
    result->success = false;
    result->message = "动作类型6失败 - 左臂xuan12运动执行失败";
    goal_handle->abort(result);
    RCLCPP_ERROR(logger_, "左臂xuan12运动执行失败！");
    return;
  }
  
  RCLCPP_INFO(logger_, "左臂xuan12已到达1.9199");
  
  if (goal_handle->is_canceling()) {
    result->success = false;
    result->message = "动作类型6已取消";
    goal_handle->canceled(result);
    RCLCPP_INFO(logger_, "动作类型6已取消");
    return;
  }
  
  // 2. 左臂运动到B_left_garb
  feedback->progress_percentage = 20.0;
  feedback->current_status = "左臂移动到B_left_garb...";
  goal_handle->publish_feedback(feedback);
  RCLCPP_INFO(logger_, "设置左臂目标姿态为B_left_garb");
  
  move_group_left_->setStartStateToCurrentState();
  move_group_left_->setNamedTarget("B_left_garb");
  
  moveit::planning_interface::MoveGroupInterface::Plan left_plan;
  bool left_success = (move_group_left_->plan(left_plan) == moveit::core::MoveItErrorCode::SUCCESS);
  
  if (!left_success) {
    result->success = false;
    result->message = "动作类型6失败 - 左臂到B_left_garb路径规划失败";
    goal_handle->abort(result);
    RCLCPP_ERROR(logger_, "左臂到B_left_garb路径规划失败！");
    return;
  }
  
  RCLCPP_INFO(logger_, "左臂到B_left_garb路径规划成功");
  
  if (goal_handle->is_canceling()) {
    result->success = false;
    result->message = "动作类型6已取消";
    goal_handle->canceled(result);
    RCLCPP_INFO(logger_, "动作类型6已取消");
    return;
  }
  
  feedback->progress_percentage = 40.0;
  feedback->current_status = "左臂正在移动...";
  goal_handle->publish_feedback(feedback);
  
  auto left_execute_result = move_group_left_->execute(left_plan);
  if (left_execute_result != moveit::core::MoveItErrorCode::SUCCESS) {
    result->success = false;
    result->message = "动作类型6失败 - 左臂到B_left_garb运动执行失败";
    goal_handle->abort(result);
    RCLCPP_ERROR(logger_, "左臂到B_left_garb运动执行失败！");
    return;
  }
  
  RCLCPP_INFO(logger_, "左臂已到达B_left_garb");
  
  // 张开左爪子
  feedback->progress_percentage = 42.0;
  feedback->current_status = "张开左爪子...";
  goal_handle->publish_feedback(feedback);
  
  auto left_msg = std_msgs::msg::Int32();
  left_msg.data = 120;
  zhuazi_left_publisher_->publish(left_msg);
  RCLCPP_INFO(logger_, "发布到 /zhuazi_left: 120 (张开)");
  
  // 等待爪子动作完成
  RCLCPP_INFO(logger_, "等待1.5秒让爪子动作完成...");
  std::this_thread::sleep_for(std::chrono::milliseconds(1500));
  
  // 左臂执行完成后，等待2秒
  feedback->progress_percentage = 45.0;
  feedback->current_status = "等待2秒让机械臂稳定...";
  goal_handle->publish_feedback(feedback);
  RCLCPP_INFO(logger_, "左臂执行完成，等待2秒...");
  std::this_thread::sleep_for(std::chrono::seconds(2));
  
  if (goal_handle->is_canceling()) {
    result->success = false;
    result->message = "动作类型6已取消";
    goal_handle->canceled(result);
    RCLCPP_INFO(logger_, "动作类型6已取消");
    return;
  }
  
  // ========== 右臂操作流程 ==========
  
  // 先更新机器人状态（包括左臂执行后的所有关节状态）
  feedback->progress_percentage = 50.0;
  feedback->current_status = "更新右臂状态...";
  goal_handle->publish_feedback(feedback);
  RCLCPP_INFO(logger_, "更新右臂起始状态为当前状态...");
  move_group_right_->setStartStateToCurrentState();
  
  // 3. 先将xuan11运动到1.9199
  feedback->progress_percentage = 55.0;
  feedback->current_status = "右臂xuan11移动到1.9199...";
  goal_handle->publish_feedback(feedback);
  RCLCPP_INFO(logger_, "将右臂xuan11设置为1.9199");
  
  move_group_right_->setStartStateToCurrentState();
  
  // 获取当前状态并修改xuan11关节
  moveit::core::RobotStatePtr current_state_right = move_group_right_->getCurrentState(10.0);
  if (!current_state_right) {
    RCLCPP_ERROR(logger_, "无法获取右臂当前机器人状态");
    result->success = false;
    result->message = "动作类型6失败 - 无法获取右臂当前状态";
    goal_handle->abort(result);
    return;
  }
  
  // 获取所有关节的当前值
  std::vector<double> joint_group_positions_right;
  const moveit::core::JointModelGroup* joint_model_group_right = 
    current_state_right->getJointModelGroup("arm_right");
  current_state_right->copyJointGroupPositions(joint_model_group_right, joint_group_positions_right);
  
  // 只修改xuan11的值
  const std::vector<std::string>& joint_names_right = joint_model_group_right->getVariableNames();
  for (size_t i = 0; i < joint_names_right.size(); i++) {
    if (joint_names_right[i] == "xuan11") {
      joint_group_positions_right[i] = 1.9199;
      RCLCPP_INFO(logger_, "找到xuan11关节，设置为1.9199");
      break;
    }
  }
  
  move_group_right_->setJointValueTarget(joint_group_positions_right);
  
  moveit::planning_interface::MoveGroupInterface::Plan xuan11_plan;
  bool xuan11_success = (move_group_right_->plan(xuan11_plan) == moveit::core::MoveItErrorCode::SUCCESS);
  
  if (!xuan11_success) {
    result->success = false;
    result->message = "动作类型6失败 - 右臂xuan11路径规划失败";
    goal_handle->abort(result);
    RCLCPP_ERROR(logger_, "右臂xuan11路径规划失败！");
    return;
  }
  
  RCLCPP_INFO(logger_, "右臂xuan11路径规划成功");
  
  auto xuan11_execute_result = move_group_right_->execute(xuan11_plan);
  if (xuan11_execute_result != moveit::core::MoveItErrorCode::SUCCESS) {
    result->success = false;
    result->message = "动作类型6失败 - 右臂xuan11运动执行失败";
    goal_handle->abort(result);
    RCLCPP_ERROR(logger_, "右臂xuan11运动执行失败！");
    return;
  }
  
  RCLCPP_INFO(logger_, "右臂xuan11已到达1.9199");
  
  if (goal_handle->is_canceling()) {
    result->success = false;
    result->message = "动作类型6已取消";
    goal_handle->canceled(result);
    RCLCPP_INFO(logger_, "动作类型6已取消");
    return;
  }
  
  // 4. 右臂运动到B_right_garb
  feedback->progress_percentage = 60.0;
  feedback->current_status = "右臂移动到B_right_garb...";
  goal_handle->publish_feedback(feedback);
  RCLCPP_INFO(logger_, "设置右臂目标姿态为B_right_garb");
  
  move_group_right_->setStartStateToCurrentState();
  move_group_right_->setNamedTarget("B_right_garb");
  
  moveit::planning_interface::MoveGroupInterface::Plan right_plan;
  bool right_success = (move_group_right_->plan(right_plan) == moveit::core::MoveItErrorCode::SUCCESS);
  
  if (!right_success) {
    result->success = false;
    result->message = "动作类型6失败 - 右臂到B_right_garb路径规划失败";
    goal_handle->abort(result);
    RCLCPP_ERROR(logger_, "右臂到B_right_garb路径规划失败！");
    return;
  }
  
  RCLCPP_INFO(logger_, "右臂到B_right_garb路径规划成功");
  
  if (goal_handle->is_canceling()) {
    result->success = false;
    result->message = "动作类型6已取消";
    goal_handle->canceled(result);
    RCLCPP_INFO(logger_, "动作类型6已取消");
    return;
  }
  
  feedback->progress_percentage = 80.0;
  feedback->current_status = "右臂正在移动...";
  goal_handle->publish_feedback(feedback);
  
  auto right_execute_result = move_group_right_->execute(right_plan);
  if (right_execute_result != moveit::core::MoveItErrorCode::SUCCESS) {
    result->success = false;
    result->message = "动作类型6失败 - 右臂到B_right_garb运动执行失败";
    goal_handle->abort(result);
    RCLCPP_ERROR(logger_, "右臂到B_right_garb运动执行失败！");
    return;
  }
  
  RCLCPP_INFO(logger_, "右臂已到达B_right_garb");
  
  // 张开右爪子
  feedback->progress_percentage = 85.0;
  feedback->current_status = "张开右爪子...";
  goal_handle->publish_feedback(feedback);
  
  auto right_msg = std_msgs::msg::Int32();
  right_msg.data = 120;
  zhuazi_right_publisher_->publish(right_msg);
  RCLCPP_INFO(logger_, "发布到 /zhuazi_right: 120 (张开)");
  
  // 等待爪子动作完成
  RCLCPP_INFO(logger_, "等待1.5秒让爪子动作完成...");
  std::this_thread::sleep_for(std::chrono::milliseconds(1500));
  
  // 动作完成
  feedback->progress_percentage = 100.0;
  feedback->current_status = "动作类型6任务完成";
  goal_handle->publish_feedback(feedback);
  
  result->success = true;
  result->message = "动作类型6执行成功 - 左右臂已到达抓取位置1";
  goal_handle->succeed(result);
  RCLCPP_INFO(logger_, "动作类型6执行成功 - 左右臂已到达抓取位置1");
}

} // namespace arm_control

